

.global arch_save_curr_context
///**
// * @brief save current task's context
// * 
// * @param to_save_old_sp the addr to save the old task's stack, usually the
// *                       address of ksp in task control block
// * @return int zero if return to the task, else means switching done.
// */
//int arch_save_curr_context(address to_save_old_sp);
arch_save_curr_context:
    push    {r4-r12, lr}
    mov     r1, #0
    mrs     r2, CPSR
    push    {r1-r2}
    str     sp, [r0]     //save current stack pointer
    //get processor coreid, switch to system stack
    mrc p15, 0, r0, c0, c0, 5
    and r0, r0, #3 
    add r0, r0, #1
    ldr sp, =gSVCStack
    add sp, sp, r0, lsl #12

    mov     r0, #0x1     //mark that in system control flow
    mov     pc, lr

.global save_curr_task_from_irq
save_curr_task_from_irq:
    pop     {lr}
    mov     r0, lr
    cps     #0x13
    push    {r0}
    cps     #0x12
    pop     {r0-r12}
    cps     #0x13
    push    {r0-r12,lr}
    cps     #0x12
    mrs     r2, spsr
    cps     #0x13
    mov     r1, #1
    push    {r1-r2}

    mrc     p15, 0, r0, c0, c0, 5
    and     r0, r0, #3              //r0=core
    ldr     r1, =gCurrTask
    ldr     r2, =gNextTask
    add     r1, r1, r0, lsl #2
    add     r2, r2, r0, lsl #2
    mov     r5, r1      // backup &gCurrTask[core]
    ldr     r6, =gSVCStack
    add     r0, r0, #1
    add     r6, r6, r0, lsl #12 //r6=gSVCStack[core]
    ldr     r3, [r1]    // r1 = &gCurrTask[core], r3 = gCurrTask[core]
    ldr     r4, [r2]    // r2 = &gNextTask[core], r4 = gNextTask[core]
    str     sp, [r3]    // save current stack pointer
    mov     sp, r6      //switch to system stack
    mov     r0, r3
    mov     r1, r4      // prepare arg for
    bl      shedule_irq_sync_status  //shedule_irq_sync_status(curr, next)
    str     r4, [r5]    // gCurrTask[core] = gNextTask[core]
    ldr     r0, [r4]    // load sp
    b       arch_switch_to_task



.global arch_switch_to_task
///**
// * @brief execute actual context switching
// * 
// * @param new_sp the new task's stak
// * @param to_save_old_sp the addr to save the old task's stack, usually the
// *                       address of ksp in task control block
// */
//void arch_switch_to_task(StackPointer new_sp);
arch_switch_to_task:
    cpsid   if
    mov     sp, r0
    pop     {r0-r1}
    msr     spsr, r1
    cmp     r0, #0
    beq     partial_type
    full_type:
    ldmfd   sp!, {r0-r12,lr,pc}^
    # full type has returned from here
    partial_type:
    ldmfd   sp!, {r4-r12,lr}
    mov     r0, #0
    movs    pc, lr